# import numpy as np
# g = 9.81
# R = 10.0
# v_sink = 3
# valid_window = 20
# T = np.array([0, 200, 0])
# Of = np.array([0, 0, 0])
# M0 = np.array([20000, 0, 2000])
# uM = (Of - M0) / np.linalg.norm(Of - M0)
# vM = 300 * uM
# F0 = np.array([17800, 0, 1800])
# v_min = 70
# v_max = 140
#
# angles1 = np.deg2rad(np.arange(-180, 180, 22.5))
# speeds1 = np.arange(70, 141, 10)
# trel1 = np.arange(0, 19, 2)
# tdet1 = np.arange(1, 7, 1)
# dt1 = 0.01
#
# dt2 = 0.002
#
# best_dur = -np.inf
# best_set = []
# cnt = 0
#
# for ia, theta in enumerate(angles1):
#     ux = np.cos(theta)
#     uy = np.sin(theta)
#
#     for iv, vF in enumerate(speeds1):
#         for ir, t_rel in enumerate(trel1):
#             Dx = F0[0] + vF * ux * t_rel
#             Dy = F0[1] + vF * uy * t_rel
#
#             for id, t_det_rel in enumerate(tdet1):
#                 t_det = t_rel + t_det_rel
#                 x_det = Dx + vF * ux * t_det_rel
#                 y_det = Dy + vF * uy * t_det_rel
#                 z_det = 1800 - 0.5 * g * t_det_rel ** 2
#
#                 ts = np.arange(t_det, t_det + valid_window + dt1, dt1)
#                 if len(ts) == 0:
#                     continue
#
#                 N = len(ts)
#                 Mx = M0[0] + vM[0] * ts
#                 My = M0[1] + vM[1] * ts
#                 Mz = M0[2] + vM[2] * ts
#
#                 Cx = np.full(N, x_det)
#                 Cy = np.full(N, y_det)
#                 Cz = z_det - v_sink * (ts - t_det)
#
#                 uxL = T[0] - Mx
#                 uyL = T[1] - My
#                 uzL = T[2] - Mz
#                 uu = uxL ** 2 + uyL ** 2 + uzL ** 2
#
#                 wx = Cx - Mx
#                 wy = Cy - My
#                 wz = Cz - Mz
#                 wu = wx * uxL + wy * uyL + wz * uzL
#
#                 lam = wu / uu
#                 lam = np.clip(lam, 0, 1)
#
#                 Px = Mx + lam * uxL
#                 Py = My + lam * uyL
#                 Pz = Mz + lam * uzL
#
#                 dx = Cx - Px
#                 dy = Cy - Py
#                 dz = Cz - Pz
#                 d = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
#
#                 mask = d <= R
#                 dm = np.diff(mask.astype(int))
#                 idx_enter = np.where(dm == 1)[0]
#                 idx_exit = np.where(dm == -1)[0]
#
#                 if len(mask) > 0 and mask[0]:
#                     idx_enter = np.concatenate(([0], idx_enter))
#                 if len(mask) > 0 and mask[-1]:
#                     idx_exit = np.concatenate((idx_exit, [len(mask) - 1]))
#
#                 dur = 0
#                 m_count = min(len(idx_enter), len(idx_exit))
#
#                 for k in range(m_count):
#                     i0 = max(0, idx_enter[k])
#                     i1 = min(len(ts) - 1, idx_exit[k])
#
#                     t0 = ts[i0]
#                     t1 = ts[i0 + 1]
#                     g0 = d[i0] - R
#                     g1 = d[i0 + 1] - R
#                     t_enter_val = t0 - g0 * (t1 - t0) / (g1 - g0) if i0 > 0 else ts[0]
#
#                     t0 = ts[i1]
#                     t1 = ts[i1 + 1]
#                     g0 = d[i1] - R
#                     g1 = d[i1 + 1] - R
#                     t_exit_val = t0 - g0 * (t1 - t0) / (g1 - g0) if i1 < len(ts) - 1 else ts[-1]
#
#                     seg = max(0, t_exit_val - t_enter_val)
#                     dur += seg
#
#                 cnt += 1
#                 if dur > best_dur:
#                     best_dur = dur
#                     best_set = [theta, vF, t_rel, t_det_rel]
#
# print(f'阶段1完成：评估了{cnt}个候选。当前最佳遮蔽时长={best_dur:.6f}s')
#
# theta0, v0, t_rel0, t_det_r0 = best_set
#
# angles2 = theta0 + np.deg2rad(np.arange(-10, 11, 2.5))
# speeds2 = np.clip(v0 + np.arange(-15, 16, 5), v_min, v_max)
# trel2 = np.clip(t_rel0 + np.arange(-0.8, 0.9, 0.2), 0, None)
# tdet2 = np.clip(t_det_r0 + np.arange(-0.8, 0.9, 0.2), 0.3, None)
#
# best2_dur = best_dur
# best2_set = best_set
#
# for theta in angles2:
#     ux = np.cos(theta)
#     uy = np.sin(theta)
#
#     for vF in speeds2:
#         for t_rel in trel2:
#             Dx = F0[0] + vF * ux * t_rel
#             Dy = F0[1] + vF * uy * t_rel
#
#             for t_det_rel in tdet2:
#                 t_det = t_rel + t_det_rel
#                 x_det = Dx + vF * ux * t_det_rel
#                 y_det = Dy + vF * uy * t_det_rel
#                 z_det = 1800 - 0.5 * g * t_det_rel ** 2
#
#                 ts = np.arange(t_det, t_det + valid_window + dt2, dt2)
#                 if len(ts) == 0:
#                     continue
#
#                 N = len(ts)
#                 Mx = M0[0] + vM[0] * ts
#                 My = M0[1] + vM[1] * ts
#                 Mz = M0[2] + vM[2] * ts
#
#                 Cx = np.full(N, x_det)
#                 Cy = np.full(N, y_det)
#                 Cz = z_det - v_sink * (ts - t_det)
#
#                 uxL = T[0] - Mx
#                 uyL = T[1] - My
#                 uzL = T[2] - Mz
#                 uu = uxL ** 2 + uyL ** 2 + uzL ** 2
#
#                 wx = Cx - Mx
#                 wy = Cy - My
#                 wz = Cz - Mz
#                 wu = wx * uxL + wy * uyL + wz * uzL
#                 lam = wu / uu
#                 lam = np.clip(lam, 0, 1)
#                 Px = Mx + lam * uxL
#                 Py = My + lam * uyL
#                 Pz = Mz + lam * uzL
#
#                 dx = Cx - Px
#                 dy = Cy - Py
#                 dz = Cz - Pz
#                 d = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
#
#                 mask = d <= R
#                 dm = np.diff(mask.astype(int))
#                 idx_enter = np.where(dm == 1)[0]
#                 idx_exit = np.where(dm == -1)[0]
#
#                 if len(mask) > 0 and mask[0]:
#                     idx_enter = np.concatenate(([0], idx_enter))
#                 if len(mask) > 0 and mask[-1]:
#                     idx_exit = np.concatenate((idx_exit, [len(mask) - 1]))
#
#                 dur = 0
#                 m_count = min(len(idx_enter), len(idx_exit))
#
#                 for k in range(m_count):
#                     i0 = max(0, idx_enter[k])
#                     i1 = min(len(ts) - 1, idx_exit[k])
#
#                     t0 = ts[i0]
#                     t1 = ts[i0 + 1]
#                     g0 = d[i0] - R
#                     g1 = d[i0 + 1] - R
#                     t_enter_val = t0 - g0 * (t1 - t0) / (g1 - g0) if i0 > 0 else ts[0]
#
#                     t0 = ts[i1]
#                     t1 = ts[i1 + 1]
#                     g0 = d[i1] - R
#                     g1 = d[i1 + 1] - R
#                     t_exit_val = t0 - g0 * (t1 - t0) / (g1 - g0) if i1 < len(ts) - 1 else ts[-1]
#
#                     seg = max(0, t_exit_val - t_enter_val)
#                     dur += seg
#
#                 if dur > best2_dur:
#                     best2_dur = dur
#                     best2_set = [theta, vF, t_rel, t_det_rel]
#
# theta, vF, t_rel, t_det_rel = best2_set
# ux = np.cos(theta)
# uy = np.sin(theta)
# Dx = F0[0] + vF * ux * t_rel
# Dy = F0[1] + vF * uy * t_rel
# Dz = 1800
# t_det = t_rel + t_det_rel
# x_det = Dx + vF * ux * t_det_rel
# y_det = Dy + vF * uy * t_det_rel
# z_det = 1800 - 0.5 * g * t_det_rel ** 2
#
# print('\n========= 问题2最优单弹遮蔽策略（FY1→M1） =========')
# print(f'航向角θ = {np.rad2deg(theta):.2f}°（相对+x逆时针）')
# print(f'飞行速度 v = {vF:.2f} m/s')
# print(f'投放时刻 t_rel = {t_rel:.3f} s')
# print(f'引信延时 t_det_rel = {t_det_rel:.3f} s')
# print(f'投放点 D = ({Dx:.3f}, {Dy:.3f}, {Dz:.3f}) m')
# print(f'起爆点 C0 = ({x_det:.3f}, {y_det:.3f}, {z_det:.3f}) m')
# print(f'起爆全局时刻 t_det = {t_det:.3f} s')
# print(f'最大遮蔽总时长 = {best2_dur:.6f} s')


import numpy as np
from numba import jit, prange

import numpy as np


# 条件二：线段穿过球体表面
def is_segment_intersect_sphere(M, P, C, R):
    dir_vec = P - M
    a = np.dot(dir_vec, dir_vec)
    if a == 0:
        return np.linalg.norm(M - C) <= R
    b = 2 * np.dot(dir_vec, M - C)
    c = np.dot(M - C, M - C) - R ** 2
    discriminant = b ** 2 - 4 * a * c
    if discriminant < 0:
        return False
    sqrt_d = np.sqrt(discriminant)
    s1 = (-b - sqrt_d) / (2 * a)
    s2 = (-b + sqrt_d) / (2 * a)
    return (0 <= s1 <= 1) or (0 <= s2 <= 1)


# 真目标采样点（50个点）
def sample_cylinder_points():
    points = []
    thetas = np.deg2rad(np.linspace(0, 360, 50, endpoint=False))
    for theta in thetas:
        x = 7 * np.cos(theta)
        y = 200 + 7 * np.sin(theta)
        points.append(np.array([x, y, 5]))
    return np.array(points)


cylinder_points = sample_cylinder_points()

# 导弹参数
M0 = np.array([20000, 0, 2000])
Of = np.array([0, 0, 0])
uM = (Of - M0) / np.linalg.norm(Of - M0)
vM = 300 * uM
missile_total_time = np.linalg.norm(Of - M0) / np.linalg.norm(vM)

# 无人机参数
F0 = np.array([17800, 0, 1800])
v_min, v_max = 70, 140
R = 10.0

# ---------------------- 全角度多阶段搜索 ----------------------
# 阶段配置：[角度步长(°), t_rel步长(s), t_det_rel步长(s)]
stages = [
    (15, 5, 2),  # 阶段1：粗搜（全角度）
    (5, 2, 1),  # 阶段2：中精度
    (1, 0.5, 0.2),  # 阶段3：高精度
    (0.1, 0.1, 0.05),  # 阶段4：超高精度
    (0.01, 0.02, 0.01)  # 阶段5：0.01°精度
]

# 初始全局搜索（阶段0：覆盖全角度）
print("===== 阶段0：全角度粗搜 =====")
angles0 = np.deg2rad(np.arange(-180, 180, 15))  # 全角度范围
speeds0 = np.arange(70, 141, 10)
trels0 = np.arange(0, 40, 10)
tdets0 = np.arange(2, 11, 2)
best_dur = -np.inf
best_set = [np.deg2rad(0), 100, 20, 5]

for theta in angles0:
    ux, uy = np.cos(theta), np.sin(theta)
    for vF in speeds0:
        for t_rel in trels0:
            Dx = F0[0] + vF * ux * t_rel
            Dy = F0[1] + vF * uy * t_rel
            for t_det_rel in tdets0:
                t_det = t_rel + t_det_rel
                if t_det <= 0 or t_det >= missile_total_time:
                    continue
                x_det = Dx + vF * ux * t_det_rel
                y_det = Dy + vF * uy * t_det_rel
                z_det = 1800 - 0.5 * 9.81 * t_det_rel ** 2
                if z_det <= 0:
                    continue
                ts = np.arange(t_det - 2, t_det + 2, 0.5)
                Mx = M0[0] + vM[0] * ts
                My = M0[1] + vM[1] * ts
                Mz = M0[2] + vM[2] * ts
                Cx, Cy = np.full(len(ts), x_det), np.full(len(ts), y_det)
                Cz = z_det - 3 * (ts - t_det)

                dur = 0
                mask_prev = False
                for i in range(len(ts)):
                    M = np.array([Mx[i], My[i], Mz[i]])
                    C = np.array([Cx[i], Cy[i], Cz[i]])
                    current_valid = False
                    for P in cylinder_points[:10]:
                        if is_segment_intersect_sphere(M, P, C, R):
                            current_valid = True
                            break
                    if current_valid:
                        if not mask_prev:
                            start = ts[i]
                            mask_prev = True
                    else:
                        if mask_prev:
                            dur += ts[i - 1] - start
                            mask_prev = False
                if mask_prev:
                    dur += ts[-1] - start
                if dur > best_dur:
                    best_dur = dur
                    best_set = [theta, vF, t_rel, t_det_rel]
                    print(f"全局粗搜最佳：{best_dur:.2f}s，角度{np.rad2deg(theta):.0f}°")

# 多阶段递进搜索
angle_range = np.deg2rad(15)  # 初始角度范围=阶段1步长
for stage, (angle_step, trel_step, tdet_step) in enumerate(stages, 1):
    theta0, vF0, t_rel0, t_det_rel0 = best_set
    # 当前阶段搜索范围（围绕最佳参数）
    angles = np.arange(theta0 - angle_range / 2, theta0 + angle_range / 2, np.deg2rad(angle_step))
    speeds = np.arange(vF0 - 10, vF0 + 11, 5)
    trels = np.arange(t_rel0 - 5, t_rel0 + 6, trel_step)
    tdets = np.arange(t_det_rel0 - 2, t_det_rel0 + 3, tdet_step)
    current_best = best_dur
    current_set = best_set

    print(f"\n===== 阶段{stage} =====")
    print(
        f"角度范围：{np.rad2deg(theta0 - angle_range / 2):.2f}°~{np.rad2deg(theta0 + angle_range / 2):.2f}°，步长{angle_step}°")

    for theta in angles:
        ux, uy = np.cos(theta), np.sin(theta)
        for vF in speeds:
            if vF < v_min or vF > v_max:
                continue
            for t_rel in trels:
                if t_rel < 0 or t_rel > missile_total_time:
                    continue
                Dx = F0[0] + vF * ux * t_rel
                Dy = F0[1] + vF * uy * t_rel
                for t_det_rel in tdets:
                    t_det = t_rel + t_det_rel
                    if t_det <= 0 or t_det >= missile_total_time:
                        continue
                    x_det = Dx + vF * ux * t_det_rel
                    y_det = Dy + vF * uy * t_det_rel
                    z_det = 1800 - 0.5 * 9.81 * t_det_rel ** 2
                    if z_det <= 0:
                        continue
                    ts = np.arange(t_det - 2, t_det + 2, 0.1)
                    Mx = M0[0] + vM[0] * ts
                    My = M0[1] + vM[1] * ts
                    Mz = M0[2] + vM[2] * ts
                    Cx, Cy = np.full(len(ts), x_det), np.full(len(ts), y_det)
                    Cz = z_det - 3 * (ts - t_det)

                    dur = 0
                    mask_prev = False
                    for i in range(len(ts)):
                        M = np.array([Mx[i], My[i], Mz[i]])
                        C = np.array([Cx[i], Cy[i], Cz[i]])
                        current_valid = False
                        for P in cylinder_points:
                            if is_segment_intersect_sphere(M, P, C, R):
                                current_valid = True
                                break
                        if current_valid:
                            if not mask_prev:
                                start = ts[i]
                                mask_prev = True
                        else:
                            if mask_prev:
                                dur += ts[i - 1] - start
                                mask_prev = False
                    if mask_prev:
                        dur += ts[-1] - start

                    if dur > current_best:
                        current_best = dur
                        current_set = [theta, vF, t_rel, t_det_rel]
                        print(f"阶段{stage}最佳：{current_best:.4f}s，角度{np.rad2deg(theta):.4f}°")

    # 更新全局最佳和下一阶段范围
    if current_best > best_dur:
        best_dur = current_best
        best_set = current_set
        print(f"阶段{stage}更新全局最佳：{best_dur:.4f}s")
    angle_range = np.deg2rad(angle_step * 2)  # 范围=2倍步长

# 最终结果
theta, vF, t_rel, t_det_rel = best_set
t_det = t_rel + t_det_rel
x_det = F0[0] + vF * np.cos(theta) * (t_rel + t_det_rel)
y_det = F0[1] + vF * np.sin(theta) * (t_rel + t_det_rel)
z_det = 1800 - 0.5 * 9.81 * t_det_rel ** 2
print("\n===== 全角度多阶段搜索最终结果 =====")
print(f"最大遮蔽时长 = {best_dur:.6f} 秒")
print(f"航向角θ = {np.rad2deg(theta):.4f}°（精度0.01°）")
print(f"投放时刻t_rel = {t_rel:.3f}s，引信延时t_det_rel = {t_det_rel:.3f}s")
print(f"起爆点位置：(x={x_det:.2f}, y={y_det:.2f}, z={z_det:.2f})m")